#include "car_control.hpp"
#include <glog/logging.h>

pid_pos *yaw_pid = nullptr;
control_node *yaw_share_node = nullptr;

pid_pos *x_pid_out = nullptr;
pid_pos *x_pid_in = nullptr;
pid_double_ring *x_pid = nullptr;
control_node *x_node = nullptr;
control_node *x_vel_node;
pid_pos *y_pid_out = nullptr;
pid_pos *y_pid_in = nullptr;
pid_double_ring *y_pid = nullptr;
control_node *y_node = nullptr;
control_node *y_vel_node;

pid_pos *y_claw_pid_out = nullptr;
pid_pos *y_claw_pid_in = nullptr;
pid_double_ring *y_claw_pid = nullptr;
control_node *claw_node = nullptr;
control_node *claw_control_node = nullptr;

std::atomic<bool> is_claw_mode_enable_at = false;

std::mutex cam_lock;
camera::camera_color_en pick_color;

camera *cam = nullptr;

control_node *car_control_node = nullptr;
yj_return car_control_init_flag = YJ_ERROR;

extern tjc_screen *screen;

void car_control_deinit(void);

yj_return car_control_init(void)
{
    screen->set_status_item(tjc_screen::tjc_screen_status_usb,
                            ("car control init fail"),
                            tjc_screen::tjc_screen_pic_small_error);
    while (car_all_init_flag != YJ_OK)
    {
        LOG(INFO) << "car_control_init wait car_all_init_flag";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    yaw_pid = new pid_pos(
        1.0f,    // Kp
        0.01f,   // Ki
        0.4f,    // Kd
        0.0f,    // 默认目标值
        500.0f,  // 积分限幅最大值
        -500.0f, // 积分限幅最小值
        150.0f,  // 输出限幅最大值
        -150.0f  // 输出限幅最小值
    );

    yaw_share_node = new control_node();

    x_pid_out = new pid_pos(
        1.0f,    // Kp
        0.08f,   // Ki
        0.0f,    // Kd
        0.0f,    // 默认目标值
        120.0f,  // 积分限幅最大值
        -120.0f, // 积分限幅最小值
        2000.0f, // 输出限幅最大值
        -2000.0f // 输出限幅最小值
    );

    x_pid_in = new pid_pos(
        1.5f,    // Kp
        0.02f,   // Ki
        0.5f,    // Kd
        0.0f,    // 默认目标值
        500.0f,  // 积分限幅最大值
        -500.0f, // 积分限幅最小值
        2000.0f, // 输出限幅最大值
        -2000.0f // 输出限幅最小值
    );

    //     0.5f,    // Kp
    // 0.01f,   // Ki
    // 0.2f,    // Kd
    // 0.0f,    // 默认目标值
    // 500.0f,  // 积分限幅最大值
    // -500.0f, // 积分限幅最小值
    // 2000.0f, // 输出限幅最大值
    // -2000.0f // 输出限幅最小值

    x_pid = new pid_double_ring(x_pid_in, x_pid_out);
    x_node = new control_node();
    x_vel_node = new control_node();

    y_pid_out = new pid_pos(
        1.0f,    // Kp
        0.0f,    // Ki
        0.1f,    // Kd
        0.0f,    // 默认目标值
        500.0f,  // 积分限幅最大值
        -500.0f, // 积分限幅最小值
        1000.0f, // 输出限幅最大值
        -1000.0f // 输出限幅最小值
    );

    y_pid_in = new pid_pos(
        1.0f,    // Kp
        0.01f,   // Ki
        0.3f,    // Kd
        0.0f,    // 默认目标值
        500.0f,  // 积分限幅最大值
        -500.0f, // 积分限幅最小值
        2000.0f, // 输出限幅最大值
        -2000.0f // 输出限幅最小值
    );

    y_pid = new pid_double_ring(y_pid_in, y_pid_out);
    y_node = new control_node();
    y_vel_node = new control_node();

    y_claw_pid_out = new pid_pos(
        3.0f,     // 5.0f,     // Kp
        0.03f,    // 0.06f,    // Ki
        0.0f,     // Kd
        0.0f,     // 默认目标值
        5000.0f,  // 积分限幅最大值
        -5000.0f, // 积分限幅最小值
        150.0f,   // 输出限幅最大值
        -150.0f   // 输出限幅最小值
    );
    // 保底
    // y_claw_pid_out = new pid_pos(
    //     3.0f,     // 5.0f,     // Kp
    //     0.03f,    // 0.06f,    // Ki
    //     0.0f,     // Kd
    //     0.0f,     // 默认目标值
    //     5000.0f,  // 积分限幅最大值
    //     -5000.0f, // 积分限幅最小值
    //     150.0f,   // 输出限幅最大值
    //     -150.0f   // 输出限幅最小值
    // );

    y_claw_pid_in = new pid_pos(
        0.8f,    // Kp
        0.02f,   // Ki
        0.3f,    // Kd
        0.0f,    // 默认目标值
        500.0f,  // 积分限幅最大值
        -500.0f, // 积分限幅最小值
        150.0f,  // 输出限幅最大值
        -150.0f  // 输出限幅最小值
    );
    // 保底
    // y_claw_pid_in = new pid_pos(
    //     0.8f,    // Kp
    //     0.02f,   // Ki
    //     0.3f,    // Kd
    //     0.0f,    // 默认目标值
    //     500.0f,  // 积分限幅最大值
    //     -500.0f, // 积分限幅最小值
    //     150.0f,  // 输出限幅最大值
    //     -150.0f  // 输出限幅最小值
    // );
    y_claw_pid = new pid_double_ring(y_claw_pid_in, y_claw_pid_out);

    claw_node = new control_node();

    claw_control_node = new control_node();

    car_control_node = new control_node();

    if (
        !yaw_pid || !yaw_share_node || !x_pid ||
        !x_node || !y_pid || !y_node || !x_pid_out ||
        !y_pid_out || !x_pid_in || !y_pid_in ||
        !y_vel_node || !x_vel_node ||
        !y_claw_pid_in || !y_claw_pid_out || !y_claw_pid ||
        !claw_node || !car_control_node || !claw_control_node)
    {
        car_control_deinit();
        return YJ_ERROR;
    }

    // int i = 2;
    // while (i--)
    // {
    try
    {
        // cam = new camera(abs(i-2));
        cam = new camera(0);
        // break;
    }
    catch (const std::exception &e)
    {
        LOG(ERROR) << "cam error: " << e.what();
        if (cam)
        {
            delete cam;
            cam = nullptr;
        }
    }
    // }

    if (!cam)
    {
        screen->set_status_item(tjc_screen::tjc_screen_status_usb,
                                ("car control init error"),
                                tjc_screen::tjc_screen_pic_small_error);
        return YJ_ERROR;
    }

    screen->set_status_item(tjc_screen::tjc_screen_status_usb,
                            ("car control init ok"),
                            tjc_screen::tjc_screen_pic_small_ok);

    screen->set_status_item(tjc_screen::tjc_screen_status_next_page,
                            " ",
                            tjc_screen::tjc_screen_pic_big_ok);

    car_control_init_flag = YJ_OK;
    return YJ_OK;
}

void car_control_deinit(void)
{
    if (x_vel_node)
    {
        delete x_vel_node;
        x_vel_node = nullptr;
    }

    if (y_vel_node)
    {
        delete y_vel_node;
        y_vel_node = nullptr;
    }

    if (yaw_pid)
    {
        delete yaw_pid;
        yaw_pid = nullptr;
    }

    if (yaw_share_node)
    {
        delete yaw_share_node;
        yaw_share_node = nullptr;
    }

    if (x_pid)
    {
        delete x_pid;
        x_pid = nullptr;
    }

    if (x_pid_in)
    {
        delete x_pid_in;
        x_pid_in = nullptr;
    }

    if (x_pid_out)
    {
        delete x_pid_out;
        x_pid_out = nullptr;
    }

    if (x_node)
    {
        delete x_node;
        x_node = nullptr;
    }

    if (y_pid)
    {
        delete y_pid;
        y_pid = nullptr;
    }

    if (y_pid_in)
    {
        delete y_pid_in;
        y_pid_in = nullptr;
    }

    if (y_pid_out)
    {
        delete y_pid_out;
        y_pid_out = nullptr;
    }

    if (y_node)
    {
        delete y_node;
        y_node = nullptr;
    }

    if (y_claw_pid_in)
    {
        delete y_claw_pid_in;
        y_claw_pid_in = nullptr;
    }

    if (y_claw_pid_out)
    {
        delete y_claw_pid_out;
        y_claw_pid_out = nullptr;
    }

    if (y_claw_pid)
    {
        delete y_claw_pid;
        y_claw_pid = nullptr;
    }

    if (claw_node)
    {
        delete claw_node;
        claw_node = nullptr;
    }

    if (car_control_node)
    {
        delete car_control_node;
        car_control_node = nullptr;
    }

    if (claw_control_node)
    {
        delete claw_control_node;
        claw_control_node = nullptr;
    }

    return;
}

void set_car_yaw_angle(double yaw)
{
    if (yaw_pid)
    {
        yaw_pid->set_des_value(yaw);
    }
}

void set_car_yaw_speed_limit(double max_speed, double min_speed)
{
    if (yaw_pid)
    {
        yaw_pid->set_output_limit(max_speed, min_speed);
    }
}

void set_car_x_mm(double x)
{

    if (x_pid)
    {
        x_pid->set_des_value(x);
    }
}

void set_car_x_speed_limit(double max_speed, double min_speed)
{
    if (x_pid)
    {
        x_pid->set_out_pid_limit_value(max_speed, min_speed);
    }
}

void set_car_y_mm(double y)
{
    if (y_pid)
    {
        y_pid->set_des_value(y);
    }
}

void set_car_y_speed_limit(double max_speed, double min_speed)
{
    if (y_pid)
    {
        y_pid->set_out_pid_limit_value(max_speed, min_speed);
    }
}

void set_car_move(double x, double y, double yaw)
{
    if (x_pid)
    {
        x_pid->set_des_value(x);
    }
    if (y_pid)
    {
        y_pid->set_des_value(y);
    }
    if (yaw_pid)
    {
        yaw_pid->set_des_value(yaw);
    }
}

void enable_claw_control(void)
{

    is_claw_mode_enable_at.store(true, std::memory_order_release);
}

void disable_claw_control(void)
{
    is_claw_mode_enable_at.store(false, std::memory_order_release);

    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    Pcar->set_bracket_vel(0, 0);
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
}

void set_claw_y_mm(double y)
{
    if (y_claw_pid)
    {
        y_claw_pid->set_des_value(y);
    }
}

void set_claw_y_speed_limit(double max_speed, double min_speed)
{
    if (y_claw_pid)
    {
        y_claw_pid->set_out_pid_limit_value(max_speed, min_speed);
    }
}

double get_car_yaw_angle(void)
{
    return yaw_pid->get_update_in_value();
}

double get_car_x_mm(void)
{
    return x_pid->get_update_in_value();
}

double get_car_y_mm(void)
{
    return y_pid->get_update_in_value();
}

// waiting_mode_car_x = 0,
// waiting_mode_car_y,
// waiting_mode_car_yaw,
// waiting_mode_claw_y,
// waiting_mode_lift,
// waiting_mode_rotate,

yj_return waiting_for_move_finish(waiting_mode_en mode, uint64_t timeout_ms, double death_zone)
{
    while (car_control_init_flag != YJ_OK)
    {
        LOG(WARNING) << "waiting for car control init ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
    yj_return ret = YJ_ERROR;

    std::this_thread::sleep_for(std::chrono::milliseconds(200));
    uint64_t start_time = yj_time_get_millitimestamp();

    switch (mode)
    {
    case waiting_mode_car_x:
        while (yj_time_get_millitimestamp() - start_time < timeout_ms)
        {
            double temp;
            temp = x_pid->get_error_value();
            if (fabs(temp) < death_zone)
                return YJ_OK;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        break;
    case waiting_mode_car_y:
        while (yj_time_get_millitimestamp() - start_time < timeout_ms)
        {
            double temp;
            temp = y_pid->get_error_value();
            if (fabs(temp) < death_zone)
                return YJ_OK;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        break;
    case waiting_mode_car_yaw:
        while (yj_time_get_millitimestamp() - start_time < timeout_ms)
        {
            double temp;
            temp = yaw_pid->get_error_value();
            if (fabs(temp) < death_zone)
                return YJ_OK;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        break;
    case waiting_mode_claw_y:
        while (yj_time_get_millitimestamp() - start_time < timeout_ms)
        {
            double temp;
            temp = y_claw_pid->get_error_value();
            if (fabs(temp) < death_zone)
                return YJ_OK;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        break;
    case waiting_mode_car_x_y_yaw:
        // LOG(INFO) << "waiting_mode_car_x_y_yaw" << "yj_time_get_millitimestamp() - start_time " << yj_time_get_millitimestamp() - start_time << " timeout_ms " << timeout_ms;

        while (yj_time_get_millitimestamp() - start_time < timeout_ms)
        {
            double temp1, temp2, temp3;
            temp1 = x_pid->get_error_value();
            temp2 = y_pid->get_error_value();
            temp3 = yaw_pid->get_error_value();

            // LOG(INFO) << "x_pid->get_error_value():" << temp1 << " y_pid->get_error_value():" << temp2 << " yaw_pid->get_error_value():" << temp3;

            if (fabs(temp1) < death_zone &&
                fabs(temp2) < death_zone &&
                fabs(temp3) < death_zone)
                return YJ_OK;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        break;
    default:
        LOG(ERROR) << "unknown mode";
        return YJ_ERROR;
        break;
    }
    return YJ_ERROR;
}

static void flow_xy_count_with_yaw(double mm_to_flow, pmw3901::pmw3901_move_data_st &data, double now_yaw_angle)
{
    static double last_yaw_angle = 0;
    double yaw_angle_step = now_yaw_angle - last_yaw_angle;
    double yaw_rad_step = yaw_angle_step * M_PI / 180.0;

    data.x_move -= -1 * sin(yaw_rad_step) * mm_to_flow;
    if (yaw_angle_step > 0)
        data.y_move += mm_to_flow * (1 - cos(yaw_rad_step));
    else
        data.y_move -= mm_to_flow * (1 - cos(yaw_rad_step));
    last_yaw_angle = now_yaw_angle;
}

void yaw_sensor_read_thread_entry()
{
    while (car_control_init_flag != YJ_OK)
    {
        LOG(WARNING) << "waiting for car control init ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    double yaw = 0;
    while (1)
    {
        hwt101_dev->read_yaw_for_pid(&yaw);
        yaw_share_node->set_w_data(-yaw);
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

void xy_sensor_read_thread_entry()
{

    while (car_control_init_flag != YJ_OK)
    {
        LOG(WARNING) << "waiting for car control init ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    pmw3901::pmw3901_move_data_st data;
    double angle = 0;
    double x_temp, y_temp;
    x_temp = y_temp = 0;
    int i = 0;
    while (1)
    {
        pmw3901_dev->count_shift_with_height(85);
        pmw3901_dev->read_move(data);

        // yaw_share_node->get_w_data(angle);
        // flow_xy_count_with_yaw(100, data, angle);

        x_temp += -1 * data.x_move;
        y_temp += data.y_move;

        // x_node->add_x_data(-1 * data.x_move);
        // y_node->add_y_data(data.y_move);

        // double x_temp, y_temp;
        // x_node->get_x_data(x_temp);
        // y_node->get_y_data(y_temp);
        if (i % 1000 == 0)
            LOG(INFO) << "flow " << "x: " << x_temp << " y: " << y_temp;
        i++;
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

void xy_sensorless_read_thread_entry()
{

    while (car_control_init_flag != YJ_OK)
    {
        LOG(WARNING) << "waiting for car control init ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    car::car_wheel_speed_t wheel_speed;
    car::car_speed_t car_speed;

    uint64_t last_time_ms = yj_time_get_millitimestamp();
    uint64_t now_time_ms = yj_time_get_millitimestamp() + 1;
    double x, y;
    while (1)
    {
        if (Pcar->read_wheel_speed(wheel_speed) != YJ_OK)
        {
            LOG(ERROR) << "read wheel speed error";
        }
        else
        {

            Pcar->cal_car_speed(wheel_speed, car_speed);

            // LOG(INFO) << " 1 spped: " << wheel_speed.wheel1_speed << " 2 speed: " << wheel_speed.wheel2_speed
            //           << " 3 speed: " << wheel_speed.wheel3_speed  << " 4 speed: " << wheel_speed.wheel4_speed;
            now_time_ms = yj_time_get_millitimestamp();
            x = car_speed.x_speed * (now_time_ms - last_time_ms) / 1000;
            y = car_speed.y_speed * (now_time_ms - last_time_ms) / 1000;
            last_time_ms = now_time_ms;

            // LOG(INFO) << "x: " << x << " y: " << y;
            x_vel_node->set_x_data(x);
            x_node->add_x_data(x);
            y_vel_node->set_y_data(y);
            y_node->add_y_data(y);

            double x_temp, y_temp;
            x_node->get_x_data(x_temp);
            y_node->get_y_data(y_temp);

            // LOG(INFO) << "x: " << x_temp << " y: " << y_temp;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

void car_yaw_control_thread_entry()
{
    while (car_control_init_flag != YJ_OK)
    {
        LOG(WARNING) << "waiting for car control init ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    double yaw_pid_value = 0;
    double yaw;
    while (1)
    {
        yaw_share_node->get_w_data(yaw);
        yaw_pid_value = yaw_pid->update_with_I_death_zone(yaw, 0.3);
        yaw_pid_value = yaw_pid->output_limit(yaw_pid_value);
        car_control_node->set_w_data(yaw_pid_value);

        // LOG(INFO) << "yaw: " << yaw;
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

void car_x_control_thread_entry()
{
    while (car_control_init_flag != YJ_OK)
    {
        LOG(WARNING) << "waiting for car control init ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    double x = 0;
    uint64_t x_time_ms = yj_time_get_millitimestamp();

    double x_last = 0;
    uint64_t x_last_time_ms = yj_time_get_millitimestamp() + 1;

    double x_velocity = 0;
    double x_pid_value = 0;
    // x_pid_out->open_debug();
    // x_pid_in->open_debug();
    // x_pid->open_debug();
    while (1)
    {
        x_node->get_x_data(x);
        x_vel_node->get_x_data(x_velocity);

        x_pid_value = x_pid->update_with_death_zone(x, x_velocity, 4);
        car_control_node->set_x_data(x_pid_value);

        // LOG(INFO) << "x: " << x << " x_velocity: " << x_velocity ;
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

void car_y_control_thread_entry()
{
    while (car_control_init_flag != YJ_OK)
    {
        LOG(WARNING) << "waiting for car control init ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    double y = 0;
    uint64_t y_time_ms = yj_time_get_millitimestamp();

    double y_last = 0;
    uint64_t y_last_time_ms = yj_time_get_millitimestamp() + 1;

    double y_velocity = 0;
    double y_pid_value = 0;
    // y_pid_out->open_debug();

    while (1)
    {
        y_node->get_y_data(y);
        y_vel_node->get_y_data(y_velocity);

        y_pid_value = y_pid->update(y, y_velocity);
        car_control_node->set_y_data(y_pid_value);

        // LOG(INFO) << "y: " << y;
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

void car_all_control_thread_entry()
{
    while (car_control_init_flag != YJ_OK)
    {
        LOG(WARNING) << "waiting for car control init ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    double x_speed, y_speed, yaw_speed, z;

    while (1)
    {

        car_control_node->get_data(x_speed, y_speed, z, yaw_speed);
        // LOG(INFO) << "x_speed " << x_speed << " y_speed " << y_speed << " yaw_speed " << yaw_speed;
        Pcar->set_speed(x_speed, y_speed, yaw_speed, 180);
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

void car_y_claw_control_thread_entry()
{
    while (car_control_init_flag != YJ_OK)
    {
        LOG(WARNING) << "waiting for car control init ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    double y_claw_pid_value = 0;
    double y_claw_speed = 0; // mm/s
    double y_claw_pos = 0;   // mm

    // y_claw_pid_in->open_debug();
    // y_claw_pid_out->open_debug();
    while (1)
    {
        bool temp = is_claw_mode_enable_at.load(std::memory_order_acquire);
        if (temp)
        {
            if (Pcar->read_bracket_speed_and_pos(y_claw_speed, y_claw_pos) != YJ_OK)
            {
                LOG(ERROR) << "read claw speed and pos error";
            }
            else
            {
                double pix_value;
                claw_node->set_y_data(y_claw_pos);
                y_claw_pid_value = y_claw_pid->update_with_death_zone(0, -y_claw_speed, 5);
                claw_control_node->set_y_data(y_claw_pid_value);

                LOG(INFO) << "y_claw_pid_value " << y_claw_pid_value << " y_claw_pos" << y_claw_pos;
                if (y_claw_pos <= -250 && y_claw_pid_value > 0)
                    Pcar->set_bracket_vel(0, 200);

                else if (y_claw_pos >= -5 && y_claw_pid_value < 0)
                    Pcar->set_bracket_vel(0, 200);
                else
                    Pcar->set_bracket_vel(y_claw_pid_value, 200);
            }
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(33));
    }
}

// 进入时填写x,y初始偏差值,在退出时会被更新为最新小车位置(等待实现)
void start_trace_color(trace_mode_en mode, camera::camera_color_en color, double is_ok_threshold)
{
    while (car_control_init_flag != YJ_OK)
    {
        LOG(WARNING) << "waiting for car control init ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    int x, y;
    double inner_p = 1;
    double i_y = 0;

    double x_offset = get_car_x_mm();
    double y_offset = get_car_y_mm();
    if (mode == trace_mode_claw_y)
        enable_claw_control();
        uint64_t start_time = yj_time_get_millitimestamp();
    while (yj_time_get_millitimestamp() -start_time < 3000)
    {

        auto center = cam->get_center(color); // 获取颜色代码为1的物体中心

        if (center)
        {
            // 计算相对于中心点的偏差
            y = center->first - 320;  // 水平偏差
            x = center->second - 240; // 垂直偏差
        }

        switch (mode)
        {
        case trace_mode_claw_y:
            set_car_x_mm(x_offset - x);
            set_claw_y_mm(y);
            break;
        case trace_mode_car_y:
            set_car_x_mm(x_offset - x);
            set_car_y_mm(y_offset - y);
            break;
        default:
            break;
        }

        // LOG(WARNING) << "x:" << x << " y:" << y;

        if (fabs(inner_p * x) < fabs(is_ok_threshold) && fabs(inner_p * y) < fabs(is_ok_threshold))
        {
            break;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(30));
    }
    if (mode == trace_mode_claw_y)
        disable_claw_control();
}

// double cam_y = braket_y - braket_y_min_abs;                         // 摄像头y轴位置
// double alpha_step = std::atan(x * x_p / cam_y);                     // 摄像头与目标点的夹角
// double alpha = rotate_angle - alpha_step;                           // 目标点绝对角度
// double new_braket_y = fabs(x / std::sin(alpha)) - braket_y_min_abs; // 摄像头与目标点的垂直距离

// if (new_braket_y < 0)
//     new_braket_y = 0;
yj_return start_trace_color_with_claw(camera::camera_color_en color, uint64_t timeout_ms, double death_zone, bool if_speed_death_zone)
{
    while (car_control_init_flag != YJ_OK)
    {
        LOG(WARNING) << "waiting for car control init ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    double error_1, error_2, error_3;
    

    uint64_t start_time = yj_time_get_millitimestamp();
    double x, y;
    double braket_y, braket_vel, braket_y_min_abs;
    double rotate_angle, rotate_speed, rotate_mid_abs;

    braket_y_min_abs = 57; // 57mm
    rotate_mid_abs = 180;

    pid_pos pid_trace_y(
        0.8,   // Kp
        0.002, // Ki
        0.16,  // Kd
        0.0,   // set_value - 目标值（中心点为0）
        80.0,  // integral_max_limit
        -80.0, // integral_min_limit
        120.0, // output_max_limit
        -120.0 // output_min_limit
    );
    yj_return flg = YJ_ERROR;



    double death_zone_buff = death_zone;
    double death_zone_set_value = death_zone;

     double   error_now ,error_last;
     double d_error_now, d_error_last;
     error_now = error_last = 0;
     d_error_now = d_error_last = 0;

     double move_detect_value = 0;


    while (yj_time_get_millitimestamp() - start_time < timeout_ms)
    {

        auto center = cam->get_center(color); // 获取颜色代码为1的物体中心

        if (!center)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(30));
            continue;
            
        }
        // 计算相对于中心点的偏差
        y = center->first - 320 - 5;   // 水平偏差
        x = center->second - 240 - 12; // 垂直偏差

        error_now = x*x + y*y;

        if(error_last != 0)
        {
            d_error_now = error_now - error_last;
        }
        error_last = error_now;

        if(d_error_last != 0)
        {
            move_detect_value = d_error_now - d_error_last;
        }

        d_error_last = d_error_now;



        if(move_detect_value != 0)
        {
            if(move_detect_value > 0) //物体在远离
            {
                 death_zone_set_value = 1;
            }
            else //物体可以抓
            {
                death_zone_set_value = death_zone_buff;
            }
        }

        

        double x_p = 0.1; // 5
        double y_p = 1.2; // 5

        double trace_y = pid_trace_y.update(y);

        Pcar->set_rotate_pos(-1 * 100 * (x * x_p) / fabs(x * x_p), fabs(x * x_p), 0, zdt_motor::motor_pos_mode_relative);
        Pcar->set_bracket_pos(-1 * 100 * (trace_y) / fabs(trace_y), fabs(trace_y), 0, zdt_motor::motor_pos_mode_relative);

        if (fabs(x) < death_zone_set_value && fabs(y) < death_zone_set_value && if_speed_death_zone == false)
        {
            flg = YJ_OK;
            break;
        }
        else if (if_speed_death_zone == true && fabs(x * x_p) < death_zone_set_value && fabs(trace_y) < death_zone_set_value)
        {
            flg = YJ_OK;
            break;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(30));
    }
    Pcar->set_bracket_pos(0, 100, 0, zdt_motor::motor_pos_mode_relative);
    Pcar->set_rotate_pos(0, 100, 0, zdt_motor::motor_pos_mode_relative);
    return flg;
}

yj_return get_claw_trace_data(double &angle, double &claw_y)
{
    double ro_angle_temp, ro_angle_speed, claw_y_temp, claw_y_speed;

    if ((Pcar->read_rotate_speed_and_pos(ro_angle_speed, angle) != YJ_OK) ||
        (Pcar->read_bracket_speed_and_pos(claw_y_speed, claw_y) != YJ_OK))
        return YJ_ERROR;
    return YJ_OK;
}
